/*
 * Math3D.h
 *
 * Created 11/29/2008 By Johnny Huynh
 *
 * Version 00.00.01 11/29/2008
 *
 * Copyright Information:
 * All content copyright © 2008 Johnny Huynh. All rights reserved.
 */
 
 // Math3D.h contains 3D math functions
 
#ifndef Math3D_H
#define Math3D_H


#include "OpenGL_Headers.h"

#include <math.h>
#include "Vector3.h"

/*** STATIC Functions declarations can be found below ***/
 template <typename TYPENAME>
 inline void normalize( TYPENAME& x, TYPENAME& y, TYPENAME& z );
 template <typename TYPENAME>
 inline void getNormalVector( TYPENAME& x, TYPENAME& y, GLboolean isCounterClockwise = true );
 template <typename TYPENAME>
 inline TYPENAME calculateAngle( TYPENAME x, TYPENAME y );
 template <typename TYPENAME>
 inline TYPENAME calculateAngle( TYPENAME x, TYPENAME y, TYPENAME radius );
 template <typename TYPENAME>
 inline TYPENAME calculateRadiusXZ( TYPENAME& normalizedY );
 template <typename TYPENAME>  
 inline TYPENAME calculateRadiusZY( TYPENAME& normalizedX );
 template <typename TYPENAME>
 inline TYPENAME calculateMagnitude( TYPENAME& x, TYPENAME& y, TYPENAME z = 0 );
 template <typename TYPENAME>
 inline Vector3<TYPENAME> nearestLinePoint( const Vector3<TYPENAME>& p, const Vector3<TYPENAME>& linePoint1, const Vector3<TYPENAME>& linePoint2 );
 template <typename TYPENAME>
 inline Vector3<TYPENAME> nearestAxisPoint( const Vector3<TYPENAME>& p, const Vector3<TYPENAME>& axisPoint1, const Vector3<TYPENAME>& axisPoint2 );
 template <typename TYPENAME>
 inline TYPENAME getRayPlaneIntersectionDistance( const Vector3<TYPENAME>& v, const Vector3<TYPENAME>& u, const Vector3<TYPENAME>& N, const Vector3<TYPENAME>& p );
 template <typename TYPENAME>
 inline bool RayPlaneIntersects( const Vector3<TYPENAME>& u, const Vector3<TYPENAME>& N );


#ifndef GL_PI
#define GL_PI 3.1415f
#endif // GL_PI

#ifndef calculateRadius
#define calculateRadius(x, y, z) calculateMagnitude(x, y, z)
#endif // calculateRadius(x, y, z)

/**
 * calculateNormalizedAngle is the same as calculateAngle() except with the
 * assumption that the radius = 1. See calculateAngle() for more details.
 *
 * @param (TYPENAME)x, (TYPENAME)y
 * @return TYPENAME
 */
#ifndef calculateNormalizedAngle
#define calculateNormalizedAngle(x, y) ( y < 0 ? -acos( x ) : acos( x ) )
#endif // calculateNormalizedAngle(x, y)

// GLOBAL FUNCTIONS

/**
 * normalize() normalizes the provided vector such that sqrt( x^2 + y^2 + z^2 ) = 1.
 *
 * @param (TYPENAME&)x, (TYPENAME&)y, (TYPENAME&)z
 */
template <typename TYPENAME>
inline void normalize( TYPENAME& x, TYPENAME& y, TYPENAME& z )
{
    TYPENAME m = calculateMagnitude(x, y, z);

    // Erroneous (infinite) number is returned if radius = 0
    // because of divide by zero
    if ( m == 0 )
    return;
    
    x /= m;
    y /= m;
    z /= m;
}

/**
 * getNormalVector() takes in the vector representing the location of an object and whether the vector
 * rotates counter-clockwise. It transforms the given x and y coordinates equivalent to rotating the
 * coordinates 90 or 270 degrees (depending on isCounterClockwise), which is orthogonal to the given x and y
 * coordinates. 
 * Note, x and y are treated as coordinates on the typical 2-dimensional x-axis and y-axis respectively.
 * Note: We assume x and y are based on the positive x-axis and y-axis.
 *
 *     y ^   
 *       |
 * -x <-- --> x
 *       |
 *    -y v 
 *
 * @param (TYPENAME&)x, (TYPENAME&)y, (GLboolean)isCounterClockwise
 * @return TYPENAME
 */
template <typename TYPENAME>
inline void getNormalVector( TYPENAME& x, TYPENAME& y, GLboolean isCounterClockwise )
{
    TYPENAME r = calculateMagnitude( x, y );
    TYPENAME angle = calculateAngle(x, y, r);
    
    if( isCounterClockwise )
        angle += GL_PI / 2.0f;   // rotate by 90 degrees
    else
        angle -= GL_PI / 2.0f;   // rotate by 270 degrees

    x = r * cos(angle);
    y = r * sin(angle);
}

/**
 * calculateAngle() calculates the angle in radians from the given location (x, y) for the 2-dimensional 
 * coordinates system. Thus, the angle is calculated as a range counter-clockwise from the given x.
 * It is assumed that the given x corresponds with the positive global x-coordinate system and the given y
 * corresponds with the positive global y-coordinate system. However, this does not mean that x and y must
 * be coordinates from the x-axis and y-axis repectively. For example, for the xz-plane, use 
 * calculateAngle( x, -z ).
 * Range: -PI < angle <= PI
 *
 *     y ^          Example for xz=plane:   -z ^   
 *       |                                     |
 * -x <-- --> x                          -x <-- --> x
 *       |                                     |
 *    -y v                                   z v
 *
 * @param (TYPENAME)x, (TYPENAME)y
 * @return TYPENAME
 */
template <typename TYPENAME>
inline TYPENAME calculateAngle( TYPENAME x, TYPENAME y )
{       
    return calculateAngle( x, y, calculateMagnitude( x, y ) );
}

/**
 * SAME AS calculateAngle() ABOVE, EXCEPT WITH THE MAGNITUDE/RADIUS CALCULATED BEFOREHAND
 * calculateAngle() calculates the angle in radians from the given location (x, y) for the 2-dimensional 
 * coordinates system. Thus, the angle is calculated as a range counter-clockwise from the given x.
 * It is assumed that the given x corresponds with the positive global x-coordinate system and the given y
 * corresponds with the positive global y-coordinate system. However, this does not mean that x and y must
 * be coordinates from the x-axis and y-axis repectively. For example, for the xz-plane, use 
 * calculateAngle( x, -z ).
 * Range: -PI < angle <= PI
 *
 *     y ^          Example for xz=plane:   -z ^   
 *       |                                     |
 * -x <-- --> x                          -x <-- --> x
 *       |                                     |
 *    -y v                                   z v
 *
 * @param (TYPENAME)x, (TYPENAME)y
 * @return TYPENAME
 */
template <typename TYPENAME>      
inline TYPENAME calculateAngle( TYPENAME x, TYPENAME y, TYPENAME radius )
{
    // Erroneous (infinite) number is returned if radius = 0
    // because of divide by zero
    if ( radius == 0 )
        return 0.0f;

    // x = r * cos( angle ).
    // Thus, angle = acos( x / r ) for accuracy from 0 to PI.
    TYPENAME angle = acos( x / radius );
    
    // fix accuracy for angle to reflect from -PI to PI
    // if y < 0 (i.e. y is negative), then angle = -angle
    if ( y < 0 )
    {
        angle = -angle;
    }
    
    return angle;
}

/**
 * calculateRadiusXZ() calculates the normalized radius on the xz-plane given the y-component of a normalized vector
 *
 * @param (TYPENAME&)normalizedY
 * @return TYPENAME
 */
template <typename TYPENAME>
inline TYPENAME calculateRadiusXZ( TYPENAME& normalizedY )
{
    // r = V cos( phi ) where -PI/2 <= phi <= PI/2. However, phi = arcsin( y / V ) and V = 1.
    // Thus, r = cos( asin( y ) ).  (Notice r will always be positive because arcsin extends from -PI/2 to PI/2 
    // and cosine of that is always positive. However, this is only true because V = 1.)
    return cos( asin( normalizedY ) );
}

/**
 * calculateRadiusZY() calculates the normalized radius on the zy-plane given the x-component of a normalized vector
 *
 * @param (TYPENAME&)normalizedX
 * @return TYPENAME
 */
template <typename TYPENAME>
inline TYPENAME calculateRadiusZY( TYPENAME& normalizedX )
{
    // r = V cos( theta ) where -PI/2 <= theta <= PI/2. However, theta = arcsin( x / V ) and V = 1.
    // Thus, r = cos( asin( x ) ).  (Notice r will always be positive because arcsin extends from -PI/2 to PI/2
    // and cosine of that is always positive. However, this is only true because V = 1.)
    return cos( asin( normalizedX ) );
}

/**
 * calculateMagnitude() calculates the magnitude/radius for the given coordinates
 *
 * @param (TYPENAME&)x, (TYPENAME&)y, (TYPENAME)z = 0, 
 * @return TYPENAME
 */
template <typename TYPENAME>
inline TYPENAME calculateMagnitude( TYPENAME& x, TYPENAME& y, TYPENAME z )
{
    return sqrt( (x*x) + (y*y) + (z*z) );
}

// Finds the nearest point on a line to the specified test point, p.
// Note: This takes in two points on the line, and it finds the nearest point 
// on the (finitely spanning) line between these two endpoints of the line.
template <typename TYPENAME>
inline Vector3<TYPENAME> nearestLinePoint( const Vector3<TYPENAME>& p, const Vector3<TYPENAME>& linePoint1, const Vector3<TYPENAME>& linePoint2 )
{
    // Let
    // A = linePoint2 - linePoint1
    // B = p - linePoint1 (not linePoint1 - p)
    // C = linePoint2 - p (not p - linePoint2)
    // nearestPoint = linePoint1 + [(A*dotProduct(A,B)) / (dotProduct(A,B) + dotProduct(A,C))]
    Vector3<TYPENAME> A = linePoint2 - linePoint1;
    Vector3<TYPENAME> B = p - linePoint1;
    Vector3<TYPENAME> C = linePoint2 - p;
    
    // ratio of the location of the nearest point on the line to linePoint1 
    // with respect to the vector A (linePoint1 to linePoint2)
    TYPENAME ratio = dotProduct(A, B) / (dotProduct(A, B) + dotProduct(A, C));
    
    // clamp ratio to be between 0 and 1
    ratio = ( ratio < 0.0f ? 0.0f : ( ratio > 1.0f ? 1.0f : ratio ) );
    
    return linePoint1 + (A*ratio);
}

// Finds the nearest point on an axis (vector) to the specified test point, p.
// Note: This takes in two points on the axis, but it finds the nearest point 
// on the (infinitely spanning) axis and not the nearest point between these 
// two endpoints of the axis.
template <typename TYPENAME>
inline Vector3<TYPENAME> nearestAxisPoint( const Vector3<TYPENAME>& p, const Vector3<TYPENAME>& axisPoint1, const Vector3<TYPENAME>& axisPoint2 )
{
    // Let
    // A = axisPoint2 - axisPoint1
    // B = p - axisPoint1 (not axisPoint1 - p)
    // C = axisPoint2 - p (not p - axisPoint2)
    // nearestPoint = axisPoint1 + [(A*dotProduct(A,B)) / (dotProduct(A,B) + dotProduct(A,C))]
    Vector3<TYPENAME> A = axisPoint2 - axisPoint1;
    Vector3<TYPENAME> B = p - axisPoint1;
    Vector3<TYPENAME> C = axisPoint2 - p;
    
    return axisPoint1 + ((A*dotProduct(A, B)) / (dotProduct(A, B) + dotProduct(A, C)));
}

 // Formula for the distance from the ray to the intersection of a plane
 // N = normal vector of the plane (unit vector or not unit vector, does not matter)
 // v = origin of the ray (location)
 // u = unit vector of the ray's shooting direction
 // d = distance from the origin of the ray to intersection of the plane (this is what we are trying to find)
 // p = a point on the plane (any point)
 //
 // N.(v + d*u) = N.p
 // d = (N.(p - v)) / (N.u)
 //
 // Notice that p is a point on the plane, which by definition, is like a vector from (0, 0, 0) 
 // to a point on the plane.
 // Notice likewise, (v + d*u) is treated as a vector from (0, 0, 0) to a point on the plane.
 // WARNING: Returns -1.0f if the line lies on the plane.
 template <typename TYPENAME>
 inline TYPENAME getRayPlaneIntersectionDistance( const Vector3<TYPENAME>& v, const Vector3<TYPENAME>& u, const Vector3<TYPENAME>& N, const Vector3<TYPENAME>& p )
 {
    return ( (N*u == 0.0f) ? -1.0f : (N*(p - v)) / (N*u) );
 }
 
 // RayPlaneIntersects() returns true if the ray and plane intersects; otherwise, false is returned.
 // N = normal vector of the plane (unit vector or not unit vector, does not matter)
 // u = vector of the ray's shooting direction
 // WARNING: Returns false if the line lies on the plane.
 template <typename TYPENAME>
 inline bool RayPlaneIntersects( const Vector3<TYPENAME>& u, const Vector3<TYPENAME>& N )
 {
    return ( (N*u == 0.0f) || (N*(p - v)) / (N*u) < 0.0f );
 }
 
 

#endif // Math3D_H